#include <iostream>
#include "midpointMethod.h"

MidpointMethod::MidpointMethod() : IntegrationMethod(){
}

MidpointMethod::MidpointMethod(double _deltaT, double _g, std::vector<State> _s0,
                               std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                               bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                               : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
}

MidpointMethod::~MidpointMethod(){
}

void MidpointMethod::calculate(bool, unsigned int _fabricPosition, double _fraction){
    //guardando dados
    State s = currentState.at(_fabricPosition);
    double t = s.getT();
    unsigned int dof = s.getDOF();
    Matrix reference = s.getReferenceMatrix();
    std::vector< std::pair<bool, Object *> > collision = s.getCollision();
    State k1, k2, aux;


    //calculando
    k1 = f(currentState.at(_fabricPosition), _fabricPosition);

    currentDeltaT = deltaT / _fraction;

    aux = (currentState.at(_fabricPosition) + (k1 * (currentDeltaT / 2.0)));
    aux.update(currentState.at(_fabricPosition), currentDeltaT / 2.0);
    k2 = f(aux, _fabricPosition);

    s = currentState.at(_fabricPosition) + (currentDeltaT * k2);

    //atualizando
    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);
    currentState.at(_fabricPosition) = s;
    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);

}
